/************************************************
 *** ARDroneV1.h
 ***
 ***   Created on: 16.05.2012
 ***       Author: Sebastian Protz <sebastian.protz@googlemail.com>
 ***	 Version: 1.0
 ***	
 *** Description:
 *** Encapsulates command interpretation from drone controller and
 *** sending instructions to the AR.Drone V1 as well as
 *** receiving navigation data from the drone.
 ***
 ************************************************/

#ifndef ARDRONEV1_H_
#define ARDRONEV1_H_


#include "ARDroneGlobalSettings.h"

#include "model/IDrone.h"

#include "Keyboard.h"
#include "ARDroneNavData.h"

using namespace boost;
using namespace asio;
using namespace ip;




#define ARDRONE_IP								"192.168.1.1"
#define ARDRONE_SEND_INTERVAL					50			// WatchDog Reset min every 200 miliseconds
#define ARDRONE_CONTROL_PORT					5556
#define ARDRONE_VIDEO_PORT						5555
#define ARDRONE_NAVIGATION_PORT 				5554
#define ARDRONE_BUFFER_SIZE						1024
#define ARDRONE_NAVDATA_BUFFER_SIZE				2048
#define ARDRONE_NAVIGATION_HEADER				0x55667788
#define ARDRONE_OPTION_SIZE						8
#define ARDRONE_NAVIGATION_USE_UNICAST 			"\x01\x00\x00\x00"
#define ARDRONE_NAVIGATION_USE_MULTICAST		"\x02\x00\x00\x00"


class ARDroneV1 : public IDrone {
	string droneModelName;

	// Commando
	char* commandBuffer;
	int32 commandBufferSaturation, commandCounter;
	bool connected, commandThread_active;
	thread* commandThread;

	// Navigation Data
	ARDroneNavData* navData;
	char* navDataBuffer;
	char* navDataReadPointer;
	bool navDataThread_active;
	int32 navDataBufferSaturation;
	thread* navDataThread;

	// read from Buffer but not used
	int16 checkSumID, checkSumSize;
	int32 checkSumData;

	void run();

	void resetWatchDog();
	void resetNavMask();
	void fTrim();
	void setNavigationOption();

	// Commands
	void animation(int32 animation, int32 duration);
	void takeOff();
	void land();
	void move(float roll, float pitch, float yaw, float vertical);
	void emergency();

	// Navigation
	void initNavDataStream();
	void receiveNavigationData();
	void decodeReceivedPackage();
	void decodeOption();
	void optionDemoTag(int16 optionSize);
	void checkDroneState();
	int16 readNext2Bytes();
	int32 readNext4Bytes();
	bool navData_droneStateConfig;

	// unknown initialization commands
	void miscellaneousCommand();
	void pModeCommand(int32 mode);

	void buildNextMovementCommandToSend();

public:
	ARDroneV1();
	virtual ~ARDroneV1();

	void connectDrone();
	void disconnectDrone();
	// Controller
	void setDroneController(IDroneController* droneController);
	IDroneController* getDroneController();
	void addDroneController(IDroneController* droneController);
	vector<IDroneController*>* getInputDeviceList();

	IDroneNavData* getNavData() const;
	IDroneVideoData* getVideoData() const;

	string getDroneModelName();
	bool isConnected();


};

#endif /* ARDRONEV1_H_ */
